#include "gec_init.h"
#include "stm32f4xx.h"
#include "gec_uart.h"
#include "stdio.h"
#include "stm32f4xx_usart.h"
#include "Delay.h"
#include "gec_gpio.h"
#include "stm32f4xx_gpio.h"
#include "CarControl.h"
#include "SearchingPath.h"
#include "BluetoothControl.h"
#include "Key.h"
#include "LED.h"
#include "PWM.h"

#define LED0 PFout(9)
#define LED1 PFout(10)
#define LED2 PEout(13)
#define LED3 PEout(14)



void  AllInit() {
  Gec_Lib_Init();
	
  Gec_UART_Init(USART1, 9600, 0, 8, 1);//usart1

	LEDInit();//初始化LED灯01234
	
	KeyInit();//初始化按键0123

  carInit();//初始化车辆

  SearchingPathInit();//初始化寻路
	
  Bluetooth_uart_init();//初始化蓝牙
	
	PWMInit();
}

int mode = 0;

int main(void) {
  AllInit();//初始化全部

  while(1) {
    //startSearchingPath();
    //startAvoidance();
    //startBluetoothControl();

    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_2) == 0) {
      mode = 1;
      LED0 = 1;
    }

    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_3) == 0) {
      mode = 2;
    }



    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_4) == 0) {
      mode = 3;
    }
		
		if(mode == 1&&(getHWLeft() == 0 || getHWRight() == 0)){//遇到障碍蜂鸣器报警
			setLEDValue(4,1);
		}else{
			setLEDValue(4,0);
		}
		
		switch(mode){
			case 0:carStop();break;
			case 1:startAvoidance();break;
			case 2:startSearchingPath();break;
			case 3:startBluetoothControl();break;
			default:;
		}
		
  }

}



/*
carGoForward();
		Delay_ms(1000);
		carGoBack();
		Delay_ms(1000);
		carTurnLeft();
		Delay_ms(1000);
		carTurnRight();
		Delay_ms(1000);
		carStop();
		Delay_ms(300);
		setLEDValue(0,0);
		Delay_ms(300);
		setLEDValue(0,1);
		Delay_ms(300);
		setLEDValue(0,2);
		Delay_ms(300);
		setLEDValue(0,3);
		Delay_ms(300);

while(1) {
    if (Gec_GPIO_Get_Value(GPIOA, GPIO_Pin_0) == 0) {
      carGoForward();
      LED0 = 0;
    }



    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_2) == 0) {
      carTurnRight();
      LED0 = 1;
    }

    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_3) == 0) {
      carTurnLeft();
    }



    if (Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_4) == 0) {
      carGoBack();
    }

    if(Gec_GPIO_Get_Value(GPIOA, GPIO_Pin_0) == 1 && Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_2) == 1 && Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_3) == 1 && Gec_GPIO_Get_Value(GPIOE, GPIO_Pin_4) == 1) {
      carStop();
    }

  }*/


/*
  int index = 0;


while(1) {
  uint8_t sw1 = Gec_GPIO_Get_Value(GPIOA,GPIO_Pin_0);
  uint8_t sw2 = Gec_GPIO_Get_Value(GPIOE,GPIO_Pin_2);
  uint8_t sw3 = Gec_GPIO_Get_Value(GPIOE,GPIO_Pin_3);
  uint8_t sw4 = Gec_GPIO_Get_Value(GPIOE,GPIO_Pin_4);

  if(sw1 == 0) {
    printf("%s  %d\n","s1 key down",index++);
    Gec_GPIO_Set_Value(GPIOF,GPIO_Pin_9,0);
    Delay_ms(500);
  } else {
    Gec_GPIO_Set_Value(GPIOF,GPIO_Pin_9,1);
  }

  if(sw2 == 0) {
    printf("%s  %d\n","s2 key down",index++);
    PFout(10) = 0;
    Delay_ms(500);
  } else {
    PFout(10) = 1;
  }


  if(sw3 == 0) {
    printf("%s  %d\n","s3 key down",index++);
    PEout(13) = 0;
    Delay_ms(500);
  } else {
    PEout(13) = 1;
  }


  if(sw4 == 0) {
    printf("%s  %d\n","s4 key down",index++);
    PEout(14) = 0;
    Delay_ms(500);
  } else {
    PEout(14) = 1;
  }



}

*/

